#include <math.h>
#include "FrictionInfoList.h"
#include "Object.h"

FrictionInfoList::FrictionInfoList()
{
	head = NULL;
}
FrictionInfoList::FrictionInfoList(FrictionInfo * headFrictionInfo)
{
	head = new List<FrictionInfo>(headFrictionInfo, NULL);
}
FrictionInfoList::~FrictionInfoList()
{
	List<FrictionInfo> * search = head;
	while(search)
	{
		search = search->Remove();
	}
	head = NULL;
}
void FrictionInfoList::Add(FrictionInfo * payload)
{
	head = new List<FrictionInfo>(payload, head);
}
void FrictionInfoList::ApplyFrictionalForce(Vector2D forceOnObject, Object * object, double elapsedTime)
{
	Vector2D force = Vector2D(0,0);
	List<FrictionInfo> * search = head;
	while(search)
	{
		if(search->payload)
		{
			Vector2D contactPoint = search->payload->GetContactPoint();
			float normalForce = search->payload->GetNormalForce().Magnitude();
			if(normalForce > 0)
			{
		
				Vector2D normal = !(search->payload->GetNormalForce());
				float axisForce = search->payload->GetAxis().dot(forceOnObject);
				
				if(fabs(search->payload->GetRelativeVelocity()) < 0.00001)
				{
						float staticForceMag = (normalForce * search->payload->GetStaticFrictionCoefficient());
						if((axisForce < 0 && staticForceMag < 0)||(axisForce > 0 && staticForceMag > 0))
							staticForceMag *= -1;
							
						if(fabs(axisForce) < fabs(staticForceMag))
							staticForceMag = -1 * axisForce;
							
						if(fabs(staticForceMag) > search->payload->GetMaxStaticFrictionForce())
						{
							if(staticForceMag < 0)
								staticForceMag = -search->payload->GetMaxStaticFrictionForce();
							else
								staticForceMag = search->payload->GetMaxStaticFrictionForce();
						}
						object->AddForceObjectSpace((search->payload->GetAxis() * staticForceMag), search->payload->GetContactPoint(), false);	 
				}
				else
				{
					float maxMag =  object->mass * (-search->payload->GetRelativeVelocity()) / elapsedTime;
					float kineticForceMag = (normalForce * search->payload->GetKineticFrictionCoefficient());

					if((kineticForceMag * maxMag) < 0)
						kineticForceMag *= -1;
					if(normalForce > 0)
					{
						if(fabs(kineticForceMag) > abs(maxMag))
						{
							kineticForceMag = maxMag;
						}
						object->AddForceObjectSpace((search->payload->GetAxis() * kineticForceMag), search->payload->GetContactPoint(), false);
					}
				}
			}
			
		}
	
		search = search->next;	
	}
}
